# AprilCalib log 2
# CalibRig::mode=2d
# @ Mon Nov 25 12:08:10 2019

from numpy import array
U=array([[286.2198486328125, 322.6930541992188, 255.5124969482422, 292.4601440429688, 222.5704345703125, 360.1233825683594, 398.2725524902344, 331.1766967773438, 259.6960144042969, 188.0071411132813, 370.9081420898438, 435.2974243164063, 299.3902587890625, 225.1532135009766, 153.9368743896484, 470.5615539550781, 409.7647705078125, 265.1738891601563, 340.2776184082031, 189.9332580566406, 380.9284362792969, 502.5138549804688, 447.0091857910156, 307.0534362792969, 229.7006530761719, 531.003173828125, 480.9974975585938, 348.9562377929688, 419.837646484375, 271.7855224609375, 455.7180786132813, 511.1788635253906, 389.4699401855469, 314.6009216308594, 487.6231384277344, 427.0424194335938, 356.3080444335938, 460.8844604492188, 395.4224243164063, 430.935546875],
       [23.90925216674805, 47.27736282348633, 50.22975921630859, 75.44985198974609, 80.70522308349609, 74.02074432373047, 103.3599472045898, 103.565055847168, 107.5805130004883, 115.045783996582, 134.4945983886719, 134.1301422119141, 137.5827331542969, 143.6397857666016, 152.1492614746094, 165.8731079101563, 166.9750213623047, 175.2227478027344, 169.9158172607422, 182.7411651611328, 203.7019195556641, 196.8778991699219, 199.6153411865234, 208.9688720703125, 215.8246154785156, 227.0769195556641, 231.5897064208984, 243.6544189453125, 237.257568359375, 250.6139373779297, 269.7062377929688, 261.8478088378906, 277.5459899902344, 285.7426147460938, 299.7361145019531, 309.754150390625, 319.5916137695313, 339.4956970214844, 351.2668151855469, 379.5458679199219]], dtype='float64');
Xw=array([[99.50446319580078, 299.5044555664063, 99.50446319580078, 299.5044555664063, 99.50446319580078, 499.5044555664063, 699.5044555664063, 499.5044555664063, 299.5044555664063, 99.50446319580078, 699.5044555664063, 899.5044555664063, 499.5044555664063, 299.5044555664063, 99.50446319580078, 1099.504516601563, 899.5044555664063, 499.5044555664063, 699.5044555664063, 299.5044555664063, 899.5044555664063, 1299.504516601563, 1099.504516601563, 699.5044555664063, 499.5044555664063, 1499.504516601563, 1299.504516601563, 899.5044555664063, 1099.504516601563, 699.5044555664063, 1299.504516601563, 1499.504516601563, 1099.504516601563, 899.5044555664063, 1499.504516601563, 1299.504516601563, 1099.504516601563, 1499.504516601563, 1299.504516601563, 1499.504516601563],
       [99.50446319580078, 99.50446319580078, 299.5044555664063, 299.5044555664063, 499.5044555664063, 99.50446319580078, 99.50446319580078, 299.5044555664063, 499.5044555664063, 699.5044555664063, 299.5044555664063, 99.50446319580078, 499.5044555664063, 699.5044555664063, 899.5044555664063, 99.50446319580078, 299.5044555664063, 699.5044555664063, 499.5044555664063, 899.5044555664063, 499.5044555664063, 99.50446319580078, 299.5044555664063, 699.5044555664063, 899.5044555664063, 99.50446319580078, 299.5044555664063, 699.5044555664063, 499.5044555664063, 899.5044555664063, 499.5044555664063, 299.5044555664063, 699.5044555664063, 899.5044555664063, 499.5044555664063, 699.5044555664063, 899.5044555664063, 699.5044555664063, 899.5044555664063, 899.5044555664063],
       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], dtype='float64');
# After LM:
K=array([[417.6498831620162, 0, 313.2414506210041],
       [0, 416.0444883479959, 260.3034796511398],
       [0, 0, 1]], dtype='float64');
distCoeffs=array([-0.3722391365340004,
       0.1177651527901426,
       -0.008627476184074889,
       -0.01013701273248732,
       0], dtype='float64');
CovK=array([[2.758942741062604, 2.799114033751272, 0.1784739296516774, 0.02546554455630762, -0.004949448433366931, 0.003314516284957844, -3.116462071316223e-05, -0.0001275602117330314, -0.0001410916189530326],
       [2.799114033751475, 2.841758816088114, 0.18882373694619, 0.026263687597924, -0.005016149984280414, 0.003357504983374531, -2.998958840356533e-05, -0.0001331800693298822, -0.0001519324570236062],
       [0.1784739296547376, 0.1888237369492832, 0.1292828289895576, 0.008456404992386953, -0.0003776223604632268, 0.0005179706570229117, 1.020988378953512e-05, -6.230404310577178e-05, -0.0004586605098851304],
       [0.02546554455688785, 0.02626368759851353, 0.008456404992433829, 0.07679194645632477, -4.80807206043275e-05, -3.856140527864121e-05, -4.045386355553276e-05, -7.609423330360976e-06, 4.318796631875366e-06],
       [-0.004949448433367779, -0.005016149984280895, -0.0003776223604579436, -4.808072060324557e-05, 1.17074365662582e-05, -1.630647269485984e-05, 5.446261236017515e-08, 2.550142532911934e-07, 1.061185662120724e-05],
       [0.003314516284962979, 0.003357504983379547, 0.0005179706570200934, -3.856140527950264e-05, -1.630647269486782e-05, 4.525366120634524e-05, 7.155405993044349e-08, -3.552171202827227e-07, -4.336322469011335e-05],
       [-3.116462071302751e-05, -2.998958840342736e-05, 1.020988378955748e-05, -4.045386355552823e-05, 5.446261235996894e-08, 7.155405993048374e-08, 3.382246277370867e-08, -2.262284975299718e-09, -8.87303460370981e-08],
       [-0.0001275602117344522, -0.000133180069331317, -6.230404310572188e-05, -7.609423330328433e-06, 2.550142532936345e-07, -3.552171202839559e-07, -2.262284975287474e-09, 3.954057005558122e-08, 3.346362160831068e-07],
       [-0.0001410916189621154, -0.000151932457032937, -0.0004586605098861084, 4.318796632094325e-06, 1.061185662122317e-05, -4.336322469012328e-05, -8.873034603719973e-08, 3.346362160836967e-07, 4.70873832983836e-05]], dtype='float64');
# rms=0.251564
r0=array([0.1594134875850517,
       0.1510846459633742,
       -1.954009169829977], dtype='float64');
t0=array([-622.6679020696519,
       670.65308020809,
       1937.371530451254], dtype='float64');
Covr0=array([[1.225555316079347e-06, 3.915762973828067e-07, 1.159829112978505e-07],
       [3.915762973806171e-07, 9.016730278150315e-07, 1.101447653829184e-07],
       [1.159829112979361e-07, 1.101447653833039e-07, 3.032502526448845e-08]], dtype='float64');
Covt0=array([[2.780849627806792, 0.3023473411463691, -3.803034637586804],
       [0.3023473411465323, 1.586720224355149, -1.699970623281556],
       [-3.803034637521621, -1.699970623268582, 58.56095123270046]], dtype='float64');
r1=array([-0.1648562659137681,
       0.0776424489199479,
       -0.03136717076807874], dtype='float64');
t1=array([-626.9751775931429,
       -616.2432748928696,
       1754.720366743008], dtype='float64');
Covr1=array([[5.476641573508294e-07, -7.217729908562999e-08, -6.51740951812624e-09],
       [-7.217729908452278e-08, 3.184307290509695e-07, 4.485038778201402e-08],
       [-6.51740951787055e-09, 4.485038778195174e-08, 1.175015478470935e-08]], dtype='float64');
Covt1=array([[2.187965421590432, 0.06837734972733571, -2.355924492056034],
       [0.06837734972869408, 1.257308403143677, 0.3339785313714656],
       [-2.35592449200388, 0.3339785313802339, 45.61302914285813]], dtype='float64');
r2=array([-0.1117457827913227,
       -0.04731541613384777,
       0.6874122414277368], dtype='float64');
t2=array([-122.8723006768039,
       -1169.955309516557,
       1609.311887476523], dtype='float64');
Covr2=array([[4.398106494258172e-07, 1.139722307445377e-07, 4.006972234255162e-09],
       [1.139722307457614e-07, 4.979939749385794e-07, 6.643130471544486e-08],
       [4.00697223445406e-09, 6.643130471548191e-08, 1.311760202549129e-08]], dtype='float64');
Covt2=array([[2.04032507895193, 0.1283918177660639, -2.71848625858342],
       [0.1283918177670289, 1.122130824434272, 0.1312039621916473],
       [-2.718486258536483, 0.1312039622003831, 40.87602854169506]], dtype='float64');
